
#include "AFMotor.h"
#include <Servo.h>
#include <Wire.h>
#include "LSM303.h"
#include "DHT.h"

#define dht_dpin A8 //kosteusanturin portti
DHT DHT_Object(dht_dpin, DHT11); //kosteusanturin nimeäminen

//renkaiden kääntöservot
Servo s1;
Servo s2;
Servo s3;
Servo s4;
//kameraservot
Servo camvaaka;
Servo campysty;
//spektron servo
Servo spektroservo;
//ajomoottorit
AF_DCMotor m1(2);
AF_DCMotor m2(1);
AF_DCMotor m3(3);
AF_DCMotor m4(4);

//magneetti- ja kiihtyvyysanturi
LSM303 compass;

//muuttujia
int w;
int w1;
int vauhti;
int rengas;
int aika = 0;
int vaakaasento;
int pystyasento;

int suora1 = 92;
int suora2 = 65;
int suora3 = 75;
int suora4 = 30;

int spektro_yla = 90;
int spektro_ala = 0;

int kaan1 = 140;
int kaan2 = 30;
int kaan3 = 40;
int kaan4 = 70;

void setup(){
  Serial1.begin(9600);
  Wire.begin(); //kompassi vaatii tämän
  delay(10);
  Serial1.println("Initializing rover datalink...");
  delay(100);
  Serial1.println("Done!");
  delay(10);
  Serial1.println("Activating motors...");
  s1.attach(33);
  s2.attach(32);
  s3.attach(35);
  s4.attach(34);
  spektroservo.attach(50);
  spektroservo.write(60);
  delay(200);
  renkaat_s();
  delay(10);
  ajo_m_n();
  delay(10);
  camsetup();
  delay(10);
  Serial1.println("Done!");
  Serial1.println("Activating accelerometer and compass...");
  compass.init();
  compass.enableDefault();
  Serial.println("Done!");
  //varmistetaan viiveellä, että kosteusanturi on vakaa
  delay(300);
  Serial1.println("Activating humidity and temperature sensor...");
  delay(700);
  Serial1.println("Done!");
  Serial1.println("LUMATech rover is ready to go!");
}

void loop(){
  ajo();
  if (aika == 0){
    kompassi();
    aurinko();
  }
  aika = aika +1;
  if (aika == 19){
    aika = 0;
  } 
}

void ajo(){
  if (Serial1.available() > 0){    //odottaa sarjaporttiliikennettä
    int inByte = Serial1.read();
    switch (inByte){
      case 'a':
        w = read_value('\n');  //seuraavat neljä on renkaiden käännön kalibrointia varten
        s1.write(w);
        suora1 = w;
        delay(200);
        break;
      case 'b':
        w = read_value('\n');
        s2.write(w);
        suora2 = w;
        delay(200);
        break;
      case 'c':
        w = read_value('\n');
        s3.write(w);
        suora3 = w;
        delay(200);
        break;
      case 'd':
        w = read_value('\n');
        s4.write(w);
        suora4 = w;
        delay(200);
        break;
      case 'A':
        w = read_value('\n');
        s1.write(w);
        kaan1 = w;
        delay(200);
        break;
      case 'B':
        w = read_value('\n');
        s2.write(w);
        kaan2 = w;
        delay(200);
        break;
      case 'C':
        w = read_value('\n');
        s3.write(w);
        kaan3 = w;
        delay(200);
        break;
      case 'D':
        w = read_value('\n');
        s4.write(w);
        kaan4 = w;
        delay(200);
        break;
      case 'e': //eteenpäin ajo
        w = read_value('\n');
        vauhti = lasku(w);
        teho(vauhti);
        m1.run(FORWARD);
        m2.run(FORWARD);
        m3.run(FORWARD);
        m4.run(FORWARD);
        break;
      case 't': //taakse ajo
        w = read_value('\n'); 
        vauhti = lasku(w);
        teho(vauhti);
        m1.run(BACKWARD);
        m2.run(BACKWARD);
        m3.run(BACKWARD);
        m4.run(BACKWARD);
        break;
      case 's': //pysäytys ja renkaat suoraan
        ajo_m_n();
        delay(10);
        renkaat_s();
        break;
      case 'v': //kääntyminen vasemmalle
        rengas = s1.read();
        if (rengas > 80 && rengas < 100){
           rkaan(); 
        }
        w = read_value('\n');
        if (w < 5){
          ajo_m_n();
          break;
        }
        vauhti = lasku(w);
        teho(vauhti);
        m1.run(BACKWARD);
        m2.run(FORWARD);
        m3.run(BACKWARD);
        m4.run(FORWARD);
        break;
      case 'o': //kääntyminen oikealle
        rengas = s1.read();
        if (rengas > 80 && rengas < 100){
          rkaan();
        }
        w = read_value('\n');
        if (w < 5){
          ajo_m_n();
          break;
        }
        vauhti = lasku(w);
        teho(vauhti);
        m1.run(FORWARD);
        m2.run(BACKWARD);
        m3.run(FORWARD);
        m4.run(BACKWARD);
        break;
      case 'h': //kameran liikuttelu vaakasuunnassa
        w = read_value('\n');
        camvaaka.write(w);
        break;
      case 'p': //kameran liikuttelu pystysuunnassa
        w = read_value('\n');
        campysty.write(w);
        break;
      case 'k': // kamera on
        camvaaka.attach(36);
        campysty.attach(37);
        break;
      case 'K': // kamera off
        camvaaka.detach();
        campysty.detach();
        break;
      case '!': //kysyy kameran asennon
        w = read_value('\n');
        vaakaasento = camvaaka.read();
        pystyasento = campysty.read();
        Serial1.print("H:");
        Serial1.println(vaakaasento);
        Serial1.print("P:");
        Serial1.println(pystyasento);
        break;
      case 'l': //lämpötilan lukeminen
        lampo();
        break;
      case 'f': //kosteus
        kosteus();
        break;
      case 'M': //spektrometrin manuaaliohjaus
        w = read_value('\n');
        spektroservo.write(w);
        spektro_yla = w;
        delay(200);
        break;
      case 'S': //spektrometrin manuaaliohjaus
        w = read_value('\n');
        spektroservo.write(w);
        spektro_ala = w;
        delay(200);
        break;
      case 'm': //spketrin mittaus
        for (w = spektro_yla; w > spektro_ala; w-=5) {
          spektroservo.write(w);
          delay(50);
        }
        spektroservo.write(spektro_ala);
        sininen();
        delay(500);
        vihrea();
        delay(500);
        punainen();
        delay(500);
        infra();
        delay(500);
        spektroservo.write(spektro_yla);
        delay(200);
        break;
      default:
        ajo_m_n();
        
    }
  }
}

int read_value(char stopper) {
    int value = 0;
    int data = -1;
    while (data != stopper) {
        data = Serial1.read();
        if (data >= '0' && data <= '9') {
            // number
            value = value*10 + (data - '0');
        }
        // anything else, including -1, gets ingnored
    }
    return value;
}

void renkaat_s(){ //renkaiden suoristus
  delay(200);
  s1.write(suora1);
  delay(100);
  s2.write(suora2);
  delay(100);
  s3.write(suora3);
  delay(100);
  s4.write(suora4);
  delay(200);
}

void ajo_m_n(){ // ajomottien perusasetus
  m1.run(RELEASE);
  m2.run(RELEASE);
  m3.run(RELEASE);
  m4.run(RELEASE);
}

void rkaan(){ //renkaat kääntymisasentoon
  s1.write(kaan1);
  delay(100);
  s2.write(kaan2);
  delay(100);
  s3.write(kaan3);
  delay(100);
  s4.write(kaan4);
  delay(200);
}

int lasku(int g){ //lasku mottien tehoa varten
  if (g < 5){     //en kyllä ymmärrä miks lähettää arvoja, jotka arduinon on jyrättävä
    g = 5;
  }
  int g1=(g*255)/10;
  return g1;
}

void teho(int g){ // ajomottien tehon säätäminen
  m1.setSpeed(g);
  m2.setSpeed(g);
  m3.setSpeed(g);
  m4.setSpeed(g);
}

void camsetup(){ //kameraservojen perusasetus
  camvaaka.attach(36);
  campysty.attach(37);
  delay(10);
  camvaaka.write(90);
  campysty.write(70);
  delay(400);
  camvaaka.detach();
  campysty.detach();
}

void kompassi(){
  compass.read();
  Serial1.print("AX:");
  Serial1.println((int)compass.a.x);
  Serial1.print("AY:");
  Serial1.println((int)compass.a.y);
  Serial1.print("AZ:");
  Serial1.println((int)compass.a.z);
  
  Serial1.print("MX:");
  Serial1.println((int)compass.m.x);
  Serial1.print("MY:");
  Serial1.println((int)compass.m.y);
  Serial1.print("MZ:");
  Serial1.println((int)compass.m.z);
}

void kosteus(){
  Serial1.print("HU:");
  Serial1.println(DHT_Object.readHumidity());
  //kosteusanturi vaatii ilmeisesti 800ms välit lukemiselle
}

void lampo(){
  Serial1.print("TE:");
  Serial1.println(DHT_Object.readTemperature());
}

void aurinko(){
  int b = 0;
  for (int i = 0; i<5;){
    int a = analogRead(A9);
    b = b + a;
    i = i+1;
  }
  int c = b/5;
  Serial1.print("AU:");
  Serial1.println(c);
}

void sininen(){
  digitalWrite(43,HIGH);
  delay(200);
  int j = 1;
  while(j < 6){
    int valo = analogRead(A10);
    Serial1.print("B:");
    Serial1.println(valo);
    j = j+1;
  }
  digitalWrite(43,LOW);
}

void vihrea(){
  digitalWrite(42,HIGH);
  delay(200);
  int j = 1;
  while(j < 6){
    int valo = analogRead(A10);
    Serial1.print("G:");
    Serial1.println(valo);
    j = j+1;
  }
  digitalWrite(42,LOW);
}

void punainen(){
  digitalWrite(47,HIGH);
  delay(200);
  int j = 1;
  while(j < 6){
    int valo = analogRead(A10);
    Serial1.print("R:");
    Serial1.println(valo);
    j = j+1;
  }
  digitalWrite(47,LOW);
}

void infra(){
  digitalWrite(49,HIGH);
  delay(200);
  int j = 1;
  while(j < 6){
    int valo = analogRead(A10);
    Serial1.print("IR:");
    Serial1.println(valo);
    j = j+1;
  }
  digitalWrite(49,LOW);
}

